
#include "LdPcl.h"

int main()
{
    float yaw = 0;
    Eigen::Vector4f Tran;
    Tran << 0, -0.2, 0, 1;
    std::vector<std::string> BroadCode;
    BroadCode.push_back("000000000000001");
    LdsLidar &mylidar = LdsLidar::GetInstance();
    int init = mylidar.InitLdsLidar(BroadCode, 40000);
    sleep(1);
    while (!init)
    {
        init = mylidar.InitLdsLidar(BroadCode, 40000);
        sleep(1);
    }
    char key;
    int count = 0;
    PcloudI OutputCloud(new CloudI());
    while(key != 'q')
    {
        PcloudI OriginCloud(new CloudI());
        mylidar.getcloud(OriginCloud);
        while (OriginCloud == nullptr || OriginCloud->size() < 40000)
        {
            sleep(0.01);
            // OriginCloud = mylidar.GetPointCloud();
            // OriginCloud = mylidar.GetPointCloud_I();
            mylidar.getcloud(OriginCloud);
        }
        
        Eigen::Matrix4f RoT;
        RoT << 
        cos(yaw),  -sin(yaw),  0,  Tran.x()*1000,
        sin(yaw),  cos(yaw),  0,  Tran.y()*1000,
        0,  0,  1,  Tran.z()*1000,
        0,  0,  0,  1;

        std::cout << RoT << std::endl;
        if(count)
        {
            Fusion(OriginCloud, OutputCloud, RoT);
        }
        else
        {
            pcl::copyPointCloud(*OriginCloud, *OutputCloud);
            count = 1;
        }
        pcl::visualization::CloudViewer V_fusion("Viewer fusion");
        V_fusion.showCloud(OutputCloud);
        while(!V_fusion.wasStopped()){}
    }
}